from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition
import os
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    go2_desc_pkg = get_package_share_directory("go2_description")
    go2_driver_pkg = get_package_share_directory("go2_driver_py")

    use_rviz = DeclareLaunchArgument(
        name="use_rviz",
        default_value="true"
    )
    
    # SLAM参数文件路径
    slam_params_file = os.path.join(
        go2_driver_pkg,
        "params",
        "slam_params.yaml"
    )

    return LaunchDescription([
        use_rviz,
        
        # URDF显示
        IncludeLaunchDescription(
            launch_description_source=PythonLaunchDescriptionSource(
                launch_file_path=os.path.join(go2_desc_pkg, "launch", "display.launch.py")
            ),
            launch_arguments=[("use_join_state_publisher", "false")]
        ),
            
        # RViz
        Node(
            package="rviz2",
            executable="rviz2",
            arguments=["-d", os.path.join(go2_desc_pkg, "rviz", "go2.rviz")],
            condition=IfCondition(LaunchConfiguration("use_rviz"))
        ),
        
        # Twist桥接
        Node(
            package="go2_twist_bridge_py",
            executable="twist_bridge"
        ),
        
        # 机器人驱动
        Node(
            package="go2_driver_py",
            executable="driver",
            parameters=[
                os.path.join(go2_driver_pkg, "params", "go2_driver.yaml")
            ]
        ),
        
        # # SLAM Toolbox节点
        # Node(
        #     package='slam_toolbox',
        #     executable='async_slam_toolbox_node',
        #     name='slam_toolbox',
        #     output='screen',
        #     parameters=[
        #         slam_params_file,  # 使用自定义参数文件
        #         {'base_frame': 'base'},  # 确保覆盖base_frame参数
        #         {'odom_frame': 'odom'}   # 明确指定odom_frame
        #     ],
        #     remappings=[
        #         ('/scan', '/utlidar/cloud'),  # 根据实际激光话题修改
        #         ('/odom', '/odom')   # 根据实际里程计话题修改
        #     ]
        # ),

        #3D点云转化为2D雷达消息
        Node(
            package='pointcloud_to_laserscan',
            executable='pointcloud_to_laserscan_node',
            name='pointcloud_to_laserscan',
            parameters=[{
                'target_frame': 'base',       # 目标坐标系
                'transform_tolerance': 0.2,       # TF变换容忍时间(s)
                'min_height': 0.0,                 # 最小处理高度(m)
                'max_height': 1.0,                 # 最大处理高度(m)
                'angle_min': -3.14159,             # -π (最小角度)
                'angle_max': 3.14159,              # +π (最大角度)
                'angle_increment': 0.0087,         # 角度增量 (0.5度)
                'scan_time': 0.1,                  # 扫描时间(s)
                'range_min': 0.1,                  # 最小有效距离(m)
                'range_max': 10.0,                 # 最大有效距离(m)
                'use_inf': True,                   # 是否使用无限值
                'inf_epsilon': 1.0                 # 无限值阈值
            }],
            remappings=[
                ('/cloud_in', '/utlidar/cloud'),  # 输入点云话题
                ('/scan', '/scan')                # 输出激光话题
            ]
        )
    ])